{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "833b377c",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "f08013fe",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 计算相对坐标，这在Chans法中是必要的\n",
    "def relative_coord(x,xref):\n",
    "    return np.float32([x[0]-xref[0],x[1]-xref[1]])\n",
    "def K(x,y):\n",
    "    return np.float32(x**2+y**2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "86fb3745",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 假定位置坐标（X,Y）表示\n",
    "x1,y1 = (1.5,3.0)\n",
    "x2,y2 = (1.0,3.0)\n",
    "x3,y3 = (2.0,3.0)\n",
    "\n",
    "# 假定到达距离差（由时间差乘上音速得到）\n",
    "# 我们这里假设声源点位为（0.5，0）\n",
    "r_21 = -0.121\n",
    "r_31 = 0.192"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "ede41173",
   "metadata": {},
   "outputs": [],
   "source": [
    "p1 = - np.linalg.pinv(np.array(\n",
    "    [[x2-x1,y2-y1],\n",
    "     [x3-x1,y3-y1]]\n",
    "     ,dtype=np.float32))\n",
    "p2 = np.array(\n",
    "    [[r_21],\n",
    "     [r_31]]\n",
    "    ,dtype=np.float32)\n",
    "k1 = K(x1,x2)\n",
    "k2 = K(x2,y2)\n",
    "k3 = K(x3,y3)\n",
    "p3 = np.array(\n",
    "    [[k1+r_21**2-k2],\n",
    "     [k1+r_31**2-k3]]\n",
    "    ,dtype=np.float32) / 2.0\n",
    "a1 = np.array(\n",
    "    [[x1],\n",
    "     [y1]]\n",
    "     ,dtype=np.float32)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "46db6323",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([[ 0.99999994, -0.99999994],\n",
       "        [-0.        , -0.        ]], dtype=float32),\n",
       " array([[-0.121],\n",
       "        [ 0.192]], dtype=float32),\n",
       " np.float32(3.25),\n",
       " np.float32(10.0),\n",
       " np.float32(13.0),\n",
       " array([[-3.3676796],\n",
       "        [-4.856568 ]], dtype=float32),\n",
       " array([[1.5],\n",
       "        [3. ]], dtype=float32))"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p1,p2,k1,k2,k3,p3,a1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "be32ad19",
   "metadata": {},
   "outputs": [],
   "source": [
    "# 求r1的二元一次方程组\n",
    "a = (p1.dot(p2).T.dot(p1.dot(p2))-1)[0][0]\n",
    "b = (p1.dot(p2).T.dot(p1.dot(p3)-a1) + (p1.dot(p3)-a1).T.dot(p1).dot(p2)\n",
    "     )[0][0]\n",
    "c = ((p1.dot(p3)-a1).T.dot(p1.dot(p3)-a1))[0][0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "3191853f",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(np.float32(-0.902031), np.float32(0.0069560953), np.float32(9.000123))"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "a,b,c"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "9e0d8051",
   "metadata": {},
   "outputs": [],
   "source": [
    "r1 = (-b-(b**2-4*a*c)**0.5)/2.0/ a\n",
    "real_coord = p1.dot(p2)*r1 + p1.dot(p3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "d274da60",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(np.float32(0.49899572), np.float32(0.0))"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "x,y = real_coord.T[0]\n",
    "x,y"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "pytorch",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
